#ifndef MODEL_INVERSE_KINEMATICS_H
#define MODEL_INVERSE_KINEMATICS_H

#include "forward_kinematics.hpp"
#include "robot_model.hpp"
#include "utilities/math.hpp"

namespace ZORAL
{
    class AnalyticalIKOfSRS
    {
    private:
        double d_bs_{};
        double d_se_{};
        double d_ew_{};
        double d_wf_{};

        Position p_02_;
        Position p_67_;

        std::shared_ptr<ForwardKinematics> fk_;

    public:
        explicit AnalyticalIKOfSRS(std::shared_ptr<ForwardKinematics> fk)
        {
            fk_ = std::move(fk);
        }

        int setGeometryParam(const double &d_bs, const double &d_se, const double &d_ew, const double &d_wf);
        double computeArmAngle(const Vector7d &joint_angles) const;
        std::vector<HomoTrans> computeAnalyticalFK(const Vector7d &joint_angles) const;
        Vector7d computeUniqueAnalyticalIK(const HomoTrans &transform_matrix, const double &arm_angle,
                                           const int &global_config) const;
        static int getGlobalConfig(const Vector7d &q);
        Vector7d computeVirtualAngles(const HomoTrans &transform_matrix, const int &GC4) const;

        // Scheme-2 helpers: expose key geometric quantities and their derivatives
        // p26 = p07 - p02 - R07 * p67 (expressed in base frame)
        Vector3d computeP26(const Vector7d &joint_angles) const;
        // 3x7 Jacobian dp26/dq using end-effector Jacobian (base-related)
        Matrix<double, 3, 7> computeP26Jacobian(const Vector7d &joint_angles) const;
        // 2x7 Jacobian of virtual angles [v1; v2] w.r.t. q, where
        // v1 = atan2(p26_y, p26_x), v2 = atan2(sqrt(px^2+py^2), p26_z) + GC4 * phi(n)
        // with n = ||p26|| and phi = acos((d_se^2 + n^2 - d_ew^2)/(2 d_se n))
        Matrix<double, 2, 7> computeVirtualAnglesJacobianWrtQ(const Vector7d &joint_angles) const;

        // Total gradient d(arm_angle)/dq for 7-DOF (centered finite difference with wrapping)
        Vector7d computeArmAngleGradient(const Vector7d &joint_angles, double step = 1e-6) const;
    };


    class InverseKinematics
    {
    private:
        std::shared_ptr<ForwardKinematics> fk_;
        std::shared_ptr<AnalyticalIKOfSRS> ik_analytical_;

    public:
        explicit InverseKinematics(std::shared_ptr<ForwardKinematics> fk)
        {
            fk_ = std::move(fk);
            ik_analytical_ = std::make_shared<AnalyticalIKOfSRS>(fk_);

            if (fk_->getRobotModel()->getRobotName() == "zoram_one_alpha")
            {
                ik_analytical_->setGeometryParam(0.155, 0.29, 0.26, 0.09);
            }
        }

        inline int getRobotDof() const
        {
            return fk_->getRobotDof();
        }

        inline std::shared_ptr<ForwardKinematics> getForwardKinematics()
        {
            return fk_;
        }

        std::shared_ptr<AnalyticalIKOfSRS> getIkAnalytical()
        {
            return ik_analytical_;
        }

        int calInverseKinematicsAnalytical(const HomoTrans &T, const double &arm_angle, const int &global_config,
                                           VectorXd &q) const;
        int calInverseKinematicsAnaSample(const HomoTrans &T, const VectorXd &q_last, VectorXd &q) const;

    };
} // namespace ZORAL
#endif